﻿using System;
using System.Text;
using System.Collections;
#if !SIMU
using GHIElectronics.NETMF.System;
using GHIElectronics.NETMF.FEZ;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.Hardware;
using Microsoft.SPOT;
using MLRobotic.Asserv.Drivers;
using MLRobotic.Asserv.Tools;
#else
using System.Diagnostics;
using MLRobotic.Simulateur._2013.Tools;
using MLRobotic.Simulateur;
#endif
using MLRobotic.IA.Tools;
using MLRobotic.IA;

namespace MLRobotic.Asserv
{
    public class Deplace
    {

        public static int codeurDroit = 0;
        private static int lastResetDroit = 0;
        public static int CodeurDroitPourObj
        {
            get { return codeurDroit - lastResetDroit; }
        }
        public static int codeurGauche = 0;
        private static int lastResetGauche = 0;
        public static int CodeurGauchePourObj
        {
            get { return codeurGauche - lastResetGauche; }
        }

        private static void ResetCodeursPourObj()
        {
            lastResetDroit = codeurDroit;
            lastResetGauche = codeurGauche;
        }

        public static double curvalDroit = 0;
        public static double curvalGauche = 0;
        public static double objAvance = 0;
        public static double objX = 0;
        public static double objY = 0;
        public static bool objEnAvant;
        public static double ObjTeta = 0;
        public static double curObjAvance = 0;
        public static double curObjAngle = 0;

        public static double ObjAvancement = 0;
        public static double ObjTetaRadian = 0;
        public static double ObjAvancementReal = 0;
        public static double ObjTetaRadianReal = 0;

        private static double acceleration = 85;//30;
        private static double accelerationAngle = 60;
        public static double currentVitesse = 0;
        private static double currentVitesseAngle = 0;
        private static double nbPasPente = 0;
        private static double nbPasPenteAngle = 0;
        public static double vitesseMax = 1000;//1500; // = Vitesse desiree/Fréquence de PID
        public static double vitesseMaxAngle = 400; // = Vitesse desiree/Fréquence de PID

        public static double coefPasMM = 18.47;

        public static int freqPWM = 20000;
        public static double Xpos = 0;
        public static double Ypos = 0;
        public static double TetaPos = 0;
        public static long NbPasGcourant, NbPasDcourant, avancement;
        static long LastCodeurG = 0;
        static long LastCodeurD = 0;
        static long EntraxePas = 5840;//5375;

        public static double Xposmm
        {
            get { return Xpos / coefPasMM; }
            set { Xpos = value * coefPasMM; }
        }
        public static double Yposmm
        {
            get { return Ypos / coefPasMM; }
            set { Ypos = value * coefPasMM; }
        }
        
#if !SIMU
        static LS7366 LSCodeurGauche, LSCodeurDroit;
       // static PWM pwmDroitTrigo, pwmDroitHoraire, pwmGaucheTrigo, pwmGaucheHoraire;
        static PWM pwmDroit, pwmGauche;


        private static OutputPort feuStopDroit, feuStopGauche, feuReculeDroit, feuReculeGauche, feuClignotantDroit, feuClignotantGauche;
        //private static OutputPort enableMotDroit, enableMotGauche;
        private static OutputPort input1Droit, input2Droit, input1Gauche, input2Gauche, standBy;
#endif

        public static void Init()
        {
            
#if !SIMU
            LSCodeurGauche = new LS7366((Cpu.Pin)FEZ_Pin.Digital.Di20, SPI.SPI_module.SPI1);
            LSCodeurDroit = new LS7366((Cpu.Pin)FEZ_Pin.Digital.Di21, SPI.SPI_module.SPI1);
            //pwmDroitTrigo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di5);
            //pwmDroitHoraire = new PWM((PWM.Pin)FEZ_Pin.PWM.Di6);
            //pwmGaucheTrigo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di9);
            //pwmGaucheHoraire = new PWM((PWM.Pin)FEZ_Pin.PWM.Di10);


            //enableMotDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di7, true);
            //enableMotGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di8, true);

            pwmDroit = new PWM((PWM.Pin)FEZ_Pin.PWM.Di5);
            pwmGauche = new PWM((PWM.Pin)FEZ_Pin.PWM.Di8);
            input1Droit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di6, true);
            input2Droit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di7, true);
            input1Gauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di9, true);
            input2Gauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di10, true);
            standBy = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di4, true);

            // Init des feux
            feuStopDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An0, false);
            feuStopGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An3, false);
            feuReculeDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An2, false);
            feuReculeGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An5, false);
            feuClignotantDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An1, false);
            feuClignotantGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An4, false);
#endif
            bufferObjAvancement = bufferObjAvancementReal = bufferObjTetaRad = bufferObjTetaReal = 0;
            nbPasPenteAngle = vitesseMaxAngle * (vitesseMaxAngle / accelerationAngle) / 2;
            // on augmente un peu la pente car sinon l'inertie casse tout
            nbPasPente = vitesseMax * (vitesseMax / acceleration) / 2 * 1.15;
#if !SIMU
            //PID.pidAvance = new PID(5, 0, 0.8, 4000, -4000, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);
            //PID.pidAngle = new PID(2, 0, 0.3, 4000, -4000, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
            PID.pidAvance = new PID(0.7, 0, 0.2, 750, -750, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);
            PID.pidAngle = new PID(0.5, 0, 0.15, 750, -750, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
#else
            PID.pidAvance = new PID(0.7, 0, 0.2, 750, -750, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);
            PID.pidAngle = new PID(0.5, 0, 0.15, 750, -750, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
#endif
            PID.Enable();
        }


        static double tP = 0.7;
        static double tI = 0;
        static double tD = 0;
        static DateTime dateDebutCalculPID = DateTime.MinValue;
        static bool premierDeplac = true;

        public static void CalculPID()
        {

            if (Deplace.ArriveACible && premierDeplac && tI < 3)
            {
                if (dateDebutCalculPID != DateTime.MinValue)
                {
                    Debug.Print(tP.ToString("N2") + ";" + tI.ToString("N2") + ";" + tD.ToString("N2") + ";" + dateDebutCalculPID.ToString("HH:mm:ss") + ";" + DateTime.Now.ToString("HH:mm:ss") + ";" + PID.pidAngle.calculPIDSommeErreur.ToString("N2"));
                }
                dateDebutCalculPID = DateTime.Now;

                tP += 0.1;
                tI += 0;
                tD += 0;

                PID.Disable();
                PID.pidAngle = null;
                PID.pidAvance = null;
                PID.pidAngle = new PID(1.7, 0, 0.25, 4000, -4000, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);
                PID.pidAvance = new PID(tP, tI, tD, 4000, -4000, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
                PID.Enable();

                // Pour test : next cible
                //GotoTeta(System.Math.PI);
                GotoXY(0, 10000, true);
                premierDeplac = false;
            }
            else if (Deplace.ArriveACible)
            {
                //GotoTeta(0);
                GotoXY(0, 0, false);
                premierDeplac = true;
            }


        }



        static bool feuxOn = false;
        static double saveCurObjAvance = 0;
        static double saveCurObjTeta = 0;
        private static double GetCurrentObjAvance()
        {
            saveCurObjAvance = curObjAvance;

            // incrémentation ou décrément de l'acceleration
            if (currentVitesse < vitesseMax
                && Math2.Abs(objAvance) > Math2.Abs(curObjAvance))
                currentVitesse += acceleration;
            else if (ObjTeta == 0 && objAvance != 0 && (currentVitesse <= 0 || Math2.Abs(objAvance) >= Math2.Abs(ObjAvancementReal)))
            {
                currentVitesse = 0;
                curObjAvance = ObjAvancementReal;
            }
            else if (currentVitesse > 0
                && Math2.Abs(objAvance) <= Math2.Abs(curObjAvance))
            {
                // arret de la correction d'angle de destination
                //objX = objY = 0;
                currentVitesse -= acceleration;
                AllumerFeuxStops();
                feuxOn = true;
            }

            if (feuxOn && currentVitesse == 0)
            {
                ArreterFeuxRecule();
                ArreterFeuxStops();
            }

            if (objAvance < 0)
                AllumerFeuxRecule();

            // on augmente de la vitesse
            curObjAvance += Math2.Sign(objAvance) * currentVitesse;



            return saveCurObjAvance;
        }


#if SIMU
        private static double aleatoireDroit = 1;
        private static double aleatoireGauche = 1;
        private static Random rand = new Random();
#endif

        private static int curVitesseDroit, curVitesseGauche;
        private static double GetCurrentAvance()
        {
            int d = codeurDroit;
            int g = codeurGauche;
            
#if !SIMU
            codeurDroit = LSCodeurDroit.ReadCounter();
            codeurGauche = -LSCodeurGauche.ReadCounter();
#else
            //aleatoireDroit *= (1.0 + rand.Next(-100, 100) / 10000.0);
            //aleatoireGauche *= (1.0 + rand.Next(-100, 100) / 10000.0);
            //if (aleatoireDroit > 1.05 || aleatoireDroit < 0.95)
            //    aleatoireDroit = 1;
            //if (aleatoireGauche > 1.05 || aleatoireGauche < 0.95)
            //    aleatoireGauche = 1;
            codeurDroit -= Convert.ToInt32(simulMotDroit  * aleatoireDroit);
            codeurGauche -= Convert.ToInt32(simulMotGauche  * aleatoireGauche);
#endif

            curVitesseDroit = System.Math.Abs(d - codeurDroit);
            curVitesseGauche = System.Math.Abs(g - codeurGauche);


            return (CodeurDroitPourObj + CodeurGauchePourObj) / 2;
        }
        private static void SetCommandAvance(double val)
        {
            // on ne settera les PWM qu'au set de l'angle (on fois qu'on aura la valeur finale)
            curvalDroit = curvalGauche = Math2.Sign(saveCurObjAvance) * currentVitesse + val;
        }

        // static string bufferLog = "";
        private static double GetCurrentObjectiveAngle()
        {
            saveCurObjTeta = curObjAngle;
            // incrémentation ou décrément de l'acceleration
            if (Math2.Abs(currentVitesseAngle) < vitesseMaxAngle
                && Math2.Abs(ObjTeta * EntraxePas / 2) > Math2.Abs(curObjAngle))
            { 
                currentVitesseAngle += accelerationAngle;
                AllumerClignotant(Math2.Sign(ObjTeta) > 0);

                //Debug.Print("Acceleration : " + ObjTetaRadianReal.ToString("F2") + " - " + ObjTetaRadian.ToString("F2") + " - " + ObjTeta.ToString("F2") + " - " + currentVitesseAngle.ToString("F2"));
            }
            else if (currentVitesseAngle < 0 || (Math2.Abs(ObjTetaRadianReal * EntraxePas / 2) <= Math2.Abs(curObjAngle)))
            {
                currentVitesseAngle = 0;
                curObjAngle = ObjTetaRadianReal * EntraxePas / 2;

                //Debug.Print("Arret : " + ObjTetaRadianReal.ToString("F2") + " - " + ObjTetaRadian.ToString("F2") + " - " + ObjTeta.ToString("F2") + " - " + currentVitesseAngle.ToString("F2"));
            }
            else if (currentVitesseAngle > 0
               && Math2.Abs(ObjTeta * EntraxePas / 2) <= Math2.Abs(curObjAngle))
            {
                currentVitesseAngle -= accelerationAngle;

                //Debug.Print("Frein : " + ObjTetaRadianReal.ToString("F2") + " - " + ObjTetaRadian.ToString("F2") + " - " + ObjTeta.ToString("F2") + " - " + currentVitesseAngle.ToString("F2"));
            }
            // cas de la correction d'angle lors de l'avance
            else if (Math2.Abs(objAvance) > 0 && ObjTeta == 0)
            {
                if (objAvance - curObjAvance > 1000)
                {
                    double ObjAv = MathEx.Sqrt(System.Math.Pow(Math2.Abs(objX - Xpos), 2) + System.Math.Pow(Math2.Abs(objY - Ypos), 2));

                    int signe = MathEx.Asin((objY - Ypos) / ObjAv) > 0 ? 1 : -1;
                    double Aacos = MathEx.Acos((objX - Xpos) / ObjAv);
                    double dteta;

                    if (objEnAvant)
                        dteta = (Aacos * signe - (TetaPos % (2 * System.Math.PI)));
                    else
                        dteta = (Aacos * signe - ((TetaPos - System.Math.PI) % (2 * System.Math.PI)));

                    if (dteta < (-System.Math.PI))
                        dteta += 2 * System.Math.PI;
                    else if (dteta > (System.Math.PI))
                        dteta -= 2 * System.Math.PI;

                   // Debug.Print("DTETA : " + dteta.ToString("F3") + "-" + objX + "-" + objY);

                    //Debug.Print("Correction : " + ObjTetaRadianReal.ToString("F2") + " - " + ObjTetaRadian.ToString("F2") + " - " + ObjTeta.ToString("F2") + " - " + currentVitesseAngle.ToString("F2"));
                    // Cas théorique
                    // return (dteta * EntraxePas * 4);
                    // en fait on ajoute un coéfficient arbitraire
                    return (dteta * EntraxePas * 0.5);

                }
                else
                {
                    //Debug.Print("Fin correction : " + ObjTetaRadianReal.ToString("F2") + " - " + ObjTetaRadian.ToString("F2") + " - " + ObjTeta.ToString("F2") + " - " + currentVitesseAngle.ToString("F2"));
                    return ObjTetaRadianReal;
                }
            }
            //else
            //    Debug.Print("Normal : " + ObjTetaRadianReal.ToString("F2") + " - " + ObjTetaRadian.ToString("F2") + " - " + ObjTeta.ToString("F2") + " - " + currentVitesseAngle.ToString("F2"));




            if (currentVitesseAngle == 0)
                ArreterClignotant();

            curObjAngle += Math2.Sign(ObjTeta) * currentVitesseAngle;

            // bufferLog += "\r\nObjTetaRadianReal;" + ObjTetaRadianReal.ToString("F4") + ";" + TetaPos.ToString("F4");


            return saveCurObjTeta;
        }

        static bool clignotantDroit = false;
        static bool clignotantGauche = false;
        private static void ArreterClignotant()
        {
            clignotantDroit = false;
            clignotantGauche = false;
        }

        private static void AllumerClignotant(bool gauche)
        {
            clignotantDroit = !gauche;
            clignotantGauche = gauche;
        }

        private static void AllumerFeuxRecule()
        {
            
#if !SIMU
            feuReculeDroit.Write(true);
            feuReculeGauche.Write(true);
#endif
        }

        private static void AllumerFeuxStops()
        {
#if !SIMU
            feuStopDroit.Write(true);
            feuStopGauche.Write(true);
#endif
        }

        private static void ArreterFeuxStops()
        {
#if !SIMU
            feuStopDroit.Write(false);
            feuStopGauche.Write(false);
#endif
        }

        private static void ArreterFeuxRecule()
        {
#if !SIMU
            feuReculeDroit.Write(false);
            feuReculeGauche.Write(false);
#endif
        }
        private static double GetCurrentCodeursAngle()
        {
            //Debug.Print("ANGLE ================= " + CodeurGauchePourObj.ToString("F2") + ";" + CodeurDroitPourObj.ToString("F2"));
            return (CodeurDroitPourObj - CodeurGauchePourObj) / 2;
        }
        private static void SetCommandAngle(double val)
        {
            // -= car lié aux branchements...
            curvalGauche -= Math2.Sign(ObjTeta) * currentVitesseAngle + val;
            curvalDroit += Math2.Sign(ObjTeta) * currentVitesseAngle + val;
            //Debug.Print("SETAVANCE:" + curvalGauche.ToString() + " - " + val.ToString());
            //Debug.Print("SET ANGLE ================= " + curvalGauche.ToString("N2") + ";" + curvalDroit.ToString("N2"));

            // Set des PWM
#if !SIMU
            //SetPWMValue(pwmGaucheTrigo, pwmGaucheHoraire, curvalGauche);
            //SetPWMValue(pwmDroitTrigo, pwmDroitHoraire, curvalDroit);

            SetPWMValue(pwmDroit, input1Droit, input2Droit, curvalDroit);
            SetPWMValue(pwmGauche, input1Gauche, input2Gauche, curvalGauche);
            Debug.Print("ASSERV;" + TetaPos.ToString("F3") + ";" + Xpos.ToString("F1") + ";" + Ypos.ToString("F1") + ";" + curvalGauche.ToString("F3") + ";" + curvalDroit.ToString("F3") + ";" + GetDutyCycle(curvalGauche) + ";" + GetDutyCycle(curvalDroit) + ";" + DateTime.Now.ToString("ssfff"));
#else
            SetMoteurs(-curvalDroit, -curvalGauche);
            Debug.Print("ASSERV;" +  curVitesseDroit.ToString() + ";" + curVitesseGauche.ToString() + ";" + curvalGauche.ToString("F3") + ";" + curvalDroit.ToString("F3") + ";" + GetDutyCycle(curvalGauche) + ";" + GetDutyCycle(curvalDroit) + ";" + DateTime.Now.ToString("mmssfff"));
#endif

        }
#if !SIMU

        public static void TestMoteurs()
        {
            //SetPWMValue(pwmGaucheTrigo, pwmGaucheHoraire, 1000);
            //SetPWMValue(pwmDroitTrigo, pwmDroitHoraire, 1000);
            SetPWMValue(pwmDroit, input1Droit,input2Droit, 1000);
            SetPWMValue(pwmGauche, input1Gauche, input2Gauche, 1000);
        }

        //private static void SetPWMValue(PWM pwmTrigo, PWM pwmHoraire, double val)
        //{
        //    if (val < 0)
        //    {
        //        pwmTrigo.Set(freqPWM, 0);
        //        pwmHoraire.Set(freqPWM, GetDutyCycle(val));

        //    }
        //    else
        //    {
        //        pwmTrigo.Set(freqPWM, GetDutyCycle(val));
        //        pwmHoraire.Set(freqPWM, 0);
        //    }
        //}
        private static void SetPWMValue(PWM pwm, OutputPort input1, OutputPort input2, double val)
        {
            if (val < 0)
            {
                pwm.Set(freqPWM, GetDutyCycle(val));
                input1.Write(true);
                input2.Write(false);
            }
            else
            {
                pwm.Set(freqPWM, GetDutyCycle(val));
                input1.Write(false);
                input2.Write(true);
            }
        }
#else
        private static double simulMotDroit = 0;
        private static double simulMotGauche = 0;
        private static void SetMoteurs(double droit, double gauche)
        {
            simulMotDroit = droit;
            simulMotGauche = gauche;
           // Debug.Print("ASSERV : " + simulMotDroit.ToString("F2") + "-" + simulMotGauche.ToString("F2") + " AVANCE : " + curObjAvance + " - " + objAvance);
        }
#endif

        private static byte GetDutyCycle(double val)
        {
            // valeur max : 1500=91% (auquel on ajoutera 9)
            byte b = System.Convert.ToByte((Math2.Abs(val) / 15).ToString("N0"));

            return (byte)(9 + (byte)(b < 91 ? b : (byte)91));
        }


        public static bool isStopped = true;
        public static Queue posHistory = new Queue();
        static DateTime dateNextCalculPosition = DateTime.Now;
        static int tempoCalculPosition = 25;
        public static void CalculPosition()
        {
            if (DateTime.Now > dateNextCalculPosition)
            {
                // Temporisation entre les calculs de position
                dateNextCalculPosition = DateTime.Now.AddMilliseconds(tempoCalculPosition);

                //Recupération du nombre de pas parcouru depuis la dernière boucle
                NbPasGcourant = codeurGauche - LastCodeurG;
                NbPasDcourant = codeurDroit - LastCodeurD;

                posHistory.Enqueue(codeurGauche);
                posHistory.Enqueue(codeurDroit);

                if (posHistory.Count >= 8)
                {
                    // on est a l'arret si les codeur n'avance pas de plus de 6 pas entre 5 ticks
                    long val = Math2.Abs((int)posHistory.Dequeue() - codeurGauche);
                    val += Math2.Abs((int)posHistory.Dequeue() - codeurDroit);
                    isStopped = currentVitesse == 0 && currentVitesseAngle == 0 && (val < 200);

                }

                LastCodeurG += NbPasGcourant;
                LastCodeurD += NbPasDcourant;


                TetaPos += (double)(NbPasDcourant - NbPasGcourant) / (double)EntraxePas;

                //calcul de l avancement effectue
                avancement = (NbPasGcourant + NbPasDcourant) / 2;

                //Mise a jour des positions
                Xpos += ((double)avancement * MathEx.Cos(TetaPos));
                Ypos += ((double)avancement * MathEx.Sin(TetaPos));
                Debug.Print("POSITION;" + Xpos.ToString("F2") + ";" + Ypos.ToString("F2") + ";" + TetaPos.ToString("F4") + ";" + NbPasGcourant.ToString("F2") + ";" + NbPasDcourant.ToString("F2") + ";" + codeurGauche.ToString("F2") + ";" + codeurDroit.ToString("F2"));
            }
        }

        static double bufferObjTetaReal, bufferObjTetaRad, bufferObjAvancement, bufferObjAvancementReal;
        public static void GotoXY(double X, double Y, bool enAvant)
        {
            Debug.Print("GOTO : " + X.ToString("F2") + "-" + Y.ToString("F2"));

            X *= coefPasMM;
            Y *= coefPasMM;

            objX = X;
            objY = Y;
            objEnAvant = enAvant;

            bufferObjAvancement = MathEx.Sqrt(System.Math.Pow(Math2.Abs(X - Xpos), 2) + System.Math.Pow(Math2.Abs(Y - Ypos), 2));


            if (bufferObjAvancement != 0)
            {
                int signe = MathEx.Asin((Y - Ypos) / bufferObjAvancement) > 0 ? 1 : -1;
                double Aacos = MathEx.Acos((X - Xpos) / bufferObjAvancement);

                if (enAvant)
                    bufferObjTetaRad = (Aacos * signe - (TetaPos % (2 * System.Math.PI)));
                else
                    bufferObjTetaRad = (Aacos * signe - ((TetaPos - System.Math.PI) % (2 * System.Math.PI)));



                if (bufferObjTetaRad < (-System.Math.PI))
                    bufferObjTetaRad += 2 * System.Math.PI;
                else if (bufferObjTetaRad > (System.Math.PI))
                    bufferObjTetaRad -= 2 * System.Math.PI;


                bufferObjTetaReal = bufferObjTetaRad;

                // suppression de l'angle du a la pente
                if (Math2.Abs(bufferObjTetaRad) > 2 * nbPasPenteAngle / EntraxePas)
                    bufferObjTetaRad += nbPasPenteAngle / EntraxePas * (bufferObjTetaRad < 0 ? 1 : -1);
                else
                    bufferObjTetaRad = bufferObjTetaRad / 2;
            }
            else
                bufferObjTetaRad = 0;


            bufferObjAvancementReal = bufferObjAvancement;
            // suppression de l'angle du a la pente
            if (Math2.Abs(bufferObjAvancement) > 2 * nbPasPente)
                bufferObjAvancement += nbPasPente * (ObjAvancement < 0 ? 1 : -1);
            else
                // on augmente un peu la pente car sinon l'inertie casse tout
                bufferObjAvancement = bufferObjAvancement / 2;

            if (!enAvant)
            {
                // on recule : avancement négatif
                bufferObjAvancement = -bufferObjAvancement;
                bufferObjAvancementReal = -bufferObjAvancementReal;
            }
#if !SIMU
            SDCard.Write("GOTO XY : " + X.ToString("F1") + ";" + Y.ToString("F1") + ";" + enAvant.ToString() + ";" + ObjTetaRadianReal.ToString() + ";" + ObjAvancementReal.ToString());
#endif
        }


        //reception d une commande de déplacement en radian en absolue
        public static void GotoTeta(double gotoTeta)
        {
#if !SIMU
            SDCard.Write("GOTO TETA : " + gotoTeta.ToString("F4"));
#endif

            // calcul de l'objectif par rapport à la position actuelle
            bufferObjTetaReal = bufferObjTetaRad = (gotoTeta - TetaPos);



            // suppression de l'angle du a la pente
            if (Math2.Abs(bufferObjTetaRad) > 2 * nbPasPenteAngle / EntraxePas)
                bufferObjTetaRad += nbPasPenteAngle / EntraxePas * (bufferObjTetaRad < 0 ? 1 : -1);
            else
                bufferObjTetaRad = bufferObjTetaRad / 2;

            Debug.Print("goto teta : " + gotoTeta.ToString("F3") + " - " + bufferObjTetaReal.ToString("F2") + " - " + bufferObjTetaRad.ToString("F2"));
        }

        public static void  Stop()
        {
#if !SIMU
            SDCard.Write("GOTO STOP");
#endif
            Debug.Print("STOP !");

            //curObjAvance = (CodeurDroitPourObj + CodeurGauchePourObj) / 2;
            //ObjTetaRadianReal = curObjAngle = (CodeurGauchePourObj - CodeurDroitPourObj) / 2;


            bufferObjAvancement = bufferObjTetaRad = ObjAvancement = 0;
            curObjAngle = (CodeurDroitPourObj - CodeurGauchePourObj) / 2;
            ObjTetaRadianReal = ObjTeta = curObjAngle * 2 / EntraxePas;
            ObjAvancementReal = objAvance = curObjAvance = (CodeurDroitPourObj + CodeurGauchePourObj) / 2;
            currentVitesse = currentVitesseAngle = 0;
            ArriveACible = false;

        }

        public static bool ArriveACible = true;
        public static void DeplaceBouge()
        {
            bool arret = true;
            arret = (ObjTeta != 0 && currentVitesseAngle == 0)
                || (objAvance != 0 && currentVitesse == 0)
                || (objAvance == 0 && ObjTeta == 0);

            //Si on est a l arret et que l on a un objectif en teta --> on tourne
            if (isStopped && arret && (bufferObjTetaRad != 0))
            {
                ObjTetaRadian = bufferObjTetaRad;
                ObjTetaRadianReal = bufferObjTetaReal;
                bufferObjTetaRad = bufferObjTetaReal = 0;

                Debug.Print("GO tourne : " + ObjTetaRadian.ToString());

                ObjTeta = ObjTetaRadian;
                ObjTetaRadian = 0;
                
#if !SIMU
                SDCard.Write("GO TETA : " + DateTime.Now.ToString("ss.fff") + ";" + ObjTeta.ToString() + ";" + Xpos.ToString() + ";" + Ypos.ToString() + ";" + TetaPos.ToString());
#endif

                Program.comPort.Send(IA.Drivers.COM.typePacket.RetourDebutRotation);

                ArriveACible = false;

                currentVitesseAngle += acceleration;

                PID.Reset();


                ResetCodeursPourObj();



                //LastCodeurD = LastCodeurG = codeurDroit = codeurGauche = 0;
                curObjAngle = curObjAvance = objAvance = 0;

            }
            // Sinon si on est à l'arret et que l'on a un objectif en distance
            else if (isStopped && arret && (bufferObjAvancement != 0))
            {

                ObjAvancement = bufferObjAvancement;
                ObjAvancementReal = bufferObjAvancementReal;
                bufferObjAvancement = bufferObjAvancementReal = 0;

                objAvance = ObjAvancement;
                ObjAvancement = 0;

         
                Program.comPort.Send(IA.Drivers.COM.typePacket.RetourFinRotation);

                Debug.Print("GO Avance : " + objAvance.ToString());
                //Log.Write("IN TETA : " + ObjTetaRadianReal.ToString() + ";" + Xpos.ToString() + ";" + Ypos.ToString() + ";" + TetaPos.ToString());

                ArriveACible = false;

                currentVitesse += acceleration;

                PID.Reset();

                ObjTetaRadianReal = (ObjTetaRadianReal * EntraxePas / 2) + ((CodeurGauchePourObj - CodeurDroitPourObj) / 2);
#if !SIMU
                SDCard.Write("GO AVANCE : " + DateTime.Now.ToString("ss.fff") + ";" + objAvance.ToString() + ";" + Xpos.ToString("F2") + ";" + Ypos.ToString("F2") + ";" + TetaPos.ToString("F4") + ";" + ObjTetaRadianReal.ToString("F4"));
#endif

                ResetCodeursPourObj();
                //LastCodeurD = LastCodeurG = codeurDroit = codeurGauche = 0;
                ObjTeta = curObjAngle = curObjAvance = 0;

            }
            else if (isStopped && arret && !ArriveACible && Math2.Abs(curVitesseDroit) + Math2.Abs(curVitesseGauche) < 3)
            {
                Program.comPort.Send(IA.Drivers.COM.typePacket.RetourFinRotation);

                // Log.Write("ARRIVE : " + DateTime.Now.ToString("ss.fff") + ";" + Xpos.ToString("F2") + ";" + Ypos.ToString("F2") + ";" + TetaPos.ToString("F4") + ";" + ObjTetaRadianReal.ToString("F4"));
                //Debug.Print("ARRET ! Arrivé");
                if (!alertDerapage)
                    Program.comPort.SendPosition(IA.Drivers.COM.typePacket.RetourPositionFinDeplace);
                if (alertDerapage)
                {
                    Program.comPort.SendPosition(IA.Drivers.COM.typePacket.RetourPositionBlocageRobot);
                    alertDerapage = false;
                }

                ArriveACible = true;

                objAvance = 0;
            }

            //// gestion du dérapage
            if ((Math2.Abs(curVitesseDroit) < 5 && Math2.Abs(curvalDroit) > 1000) || (Math2.Abs(curVitesseGauche) < 5 && Math2.Abs(curvalGauche) > 1000))
                countDerapage++;
            else
                countDerapage = 0;

            if (countDerapage >= 5 && !alertDerapage)
            {
                Debug.Print("BLOCAGE !!!!!!!");
                countDerapage = 0;
                alertDerapage = true;
                Stop();
            }

            // Gestion des clignotants
            if (nextCligno < DateTime.Now)
            {
#if !SIMU
                feuClignotantDroit.Write(clignotantDroit && clignoOn);
                feuClignotantGauche.Write(clignotantGauche && clignoOn);
#endif

                clignoOn = !clignoOn;
                nextCligno = DateTime.Now.AddMilliseconds(200);
            }
        }
        static int countDerapage = 0;
        public static bool alertDerapage = false;
        static bool clignoOn = false;
        static DateTime nextCligno = DateTime.Now;
    }
}
